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ABSTRACT 


Localization in an autonomous mobile robot allows it to operate 
autonomously in an unknown and unpredictable environment with the ability 
to determine its position and heading. Simultaneous localization and 
mapping (SLAM) are introduced to solve the problem where no prior 
information about the environment is available either static or dynamic to 
achieve standard map-based localization. The primary focus of this research 
is autonomous mobile robot navigation using the extended Kalman filter 
(EKF)-SLAM environment modeling technique which provides higher 
accuracy and reliability in mobile robot localization and mapping results. In 
this paper, EKF-SLAM performance is verified by simulations performed in 
a static and dynamic environment designed in V-REP 1e., 3D Robot 
simulation environment. In this work SLAM problem of two-wheeled 
differential drive robot i.e., Pioneer 3-DX in indoor static and dynamic 
environment integrated with Laser range finder 1.e., Hokuyo URG-04LX- 
UG01, LIDAR, and Ultrasonic sensors is solved. EKF-SLAM scripts are 


developed using MATLAB that is linked to V-REP via remote API feature to 
evaluate EKF-SLAM performance. The reached results confirm the EKF- 
SLAM is a reliable approach for real-time autonomous navigation for mobile 
robots in comparison to other techniques. 
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1. INTRODUCTION 

Autonomous mobile robots’ navigation is considerably a matter of contention for the last two 
decades. researchers and scientists are putting prodigious efforts to find reliable solution with the help of 
artificial intelligence and computer vision. The applications of mobile robots are proficient with a degree of 
self-sufficiency and the criteria of its autonomy are sensing the environment, self-maintenance, task 
performing, autonomous navigation that includes indoor and outdoor navigation while in cluttered 
environments reliable data association is important [1] because wrong data association can have catastrophic 
consequences. 

Current simultaneous localization and mapping (SLAM) approaches include the graph-SLAM 
method, particle filtering method (PF), scan matching method (SM), and the extended Kalman filter method 
(EKF). The graph-SLAM method takes the SLAM problem as an optimization problem [2]. The objective 
function and constraints are defined, and it is solved using the mathematical programming method. The PF 
method is a recursive Bayesian estimator for dynamic Bayesian networks that approximates the PDF te. 
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probability density function by searching for the cluster of random samples, uses the sample mean to replace 
the integral operations, and acquires the variance distribution of the state [3]. It has a special advantage in 
dealing with status filtering of parameter estimation of non-Gaussian and nonlinear time-varying systems 
while the major drawback is in high-dimensional spaces sampling that can be inefficient. The SM method 
minimizes the metric distances between the scan characteristics or original data, and aligns the overlap of the 
scanning datasets [4]. The SLAM process leads to better navigation in dynamic environments autonomously. 
However, a lack of proprioceptive feedback from an autonomous mobile robot may cause trouble in the 
autonomous operation of the robot in unpredictable and unknown environments. Above mentioned issues are 
some basic problems of SLAM. Nowadays, many researchers analyze different approaches to solving 
problems in SLAM and it is analyzed that the approach based on EKF among all approaches are the most 
commonly used to solve SLAM problems. Teslić et al. [5] uses LabMate™.to show robot navigation in 
indoor environment with integrated use of laser rangefinder sensor. 

EKF-SLAM is one of the probabilistic approaches to control uncertainties of any mobile robot 
[6]-[8] is most widely used in mobile robotics, especially in SLAM. Moreover, the SLAM filtering solution, 
which is based on the EKF application is the first successfully implemented [9], [10], and most often used 
online SLAM algorithm. There are many theoretical and practical works dedicated to EKF usage with 
different approaches [11], [12], and application fields [13], [14]. In the autonomous robot navigation, the 
EKF based SLAM is categorized as non-linear SLAM, where linearization of non-linear models along with a 
summation of system noise with Gaussian filter takes place so that the Kalman filter algorithm can be 
applied. In comparison with the Kalman filter, EKF-SLAM represents the non-linear models which are an 
essential part of all navigation problems almost. EKF-SLAM estimates mobile robot location as it is moving 
using incremental maximum likelihood estimator, it generates the map by localization and generated map 
information uses to update its current states and map simultaneously [15]. 

In this work, the EKF-SLAM algorithm has been successfully implemented using a pioneer 3-DX 
i.e., a small lightweight two-wheel differential drives mobile robot. The algorithm is implemented and its 
performance is analyzed in a clustered environment created on V-REP. Lidar sensor interfaced with robot 
model and the output of robot motors, sensors are then integrated with MATLAB to apply the algorithm and 
observe its performance in a static and dynamic environment. 


2. RESEARCH METHOD 

The mobile robot model predicts the current states based on control input and previous states of the 
robot is shown in Figure 1. Mathematically, the process model in the discrete form can be written as (1). The 
robot position (x, y) and orientation angle i.e., © estimated the robot states in this research work. It can be 
shown in the form of a state vector as (2). In this research work speed and angular velocity is applied as 
control input can be exhibit as in (3). 





Figure 1. Robot model 


Rm=+1) = f (Ray Un) (1) 
x 
R= v (2) 
0 
Vn) 
Un) = ono (3) 
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In discrete-time, iteratively for every n® sample, the predictive state of the robot is predicted in 
sample time T, which capture in time (n. T). 

Since the control input to the robot models are speed and angular velocity that is measured and used 
to predict robot states in this process. The discrete-time robot kinematics model for the above case is 
expressed in (4). 


X(n+1)] TŠ) Vín): COSCO n) ) 
Rn+1) = ye» e +T V(n): sin(6(ny) (4) 
8m+1)) LO) Wn) 





Robot model Jacobian can be represented in (5). 


Vn): cos(R(3) jn) 
R(n41)|n = R(n)jn +T V(n): sin(R(3) nn) (5) 
O(n) 


Ideally, the current states of the robot can be estimated accurately by using the above robot 
kinematics model but in practice, it is influenced by errors that can be noise in the measurement of sensors 
and friction. Equation (1) with the addition of the noise model can be written as (6). 


Rasy = f (Rin Umm) + Sum) ) = f (Rin Umm) + 8f (6) 


In the above case, the noise model is estimated as Gaussian noise with zero mean represented by óf. 
The covariance of this system noise originated during the measurement process can be calculated as it can be 
seen in (7). 


Qf = Filok (7) 


In which: 
Qf = Covariance of Process noise 
Qu = Covariance of Control input measurement 
Fu = ô f Ô (V,@) 
The process model Jacobian matrices concerning the control input measurement Fu and robot states 
Frv can be expressed in (8) and (9), respectively. 








1 0 -T.v sin(O m) 
Fis sayo = |0 1 T.V. cos(O)) (8) 
0 0 1 
o T. cos(R(3)m) 0 
ho So = |T. sin(R(3) m) 0 (9) 
0 T 


2.1. SLAM operation based on 2D-EKF 

The SLAM process flow in this research work can be observed in Figure 2. In contrast to EKF 
localization SLAM operation involves the initialization of landmarks to update robot position. 

The states during the process that are estimated consist of estimated landmarks and robot states. 
Point landmarks are used in this research, which has two-dimensional (x and y) states. Equation (10) exhibits 
the SLAM operation based whole estimated states vector. The mobile robot states (x, y, 0) are correlated by 
Ry and set of landmark states (Ly pEr Ly, sby,5--+) Ly, by) are correlated by R,, with n is the registered 
landmark number. Similar to the localization process, in which EKF is used to estimate the states and 
Gaussian variables including covariance matrix P and mean 1.e., the expected value of state vector is used to 
model all the processes. The correspondent of the expected value of the state vector and its covariance matrix 
P can be shown in (11) and (12). These two matrices expand every time in size with the initialization of the 
landmark process, whenever the mobile robot detects a new landmark. 
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Figure 2. 2D EKF-SLAM operation 
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2.2. 2D EKF-based SLAM process general operation 
2.2.1. Step 0. robot initialization 

At the initial time (t=0), only the robot states are initial states, and no landmark is registered to the 
map. Although, P = PO can be considered as the covariance matrix as the robot initial state is assumed to be 
known so that there is no uncertainty. 
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X 0 0 0 0 
R= Ry= | = ol P = o 0 o (14) 
6 0 0 0 0 


2.2.2. Step 1. prediction step 
a. Updating robot states 

The mobile robot movement affects the robot states only and based on the robot process model and 
its movement, the new estimated robot state (Ry) as in (15) and the landmark state (R,) as in (16) can be 
predicted. 


Ry © fr, Ry uN) (15) 


Ry e Ry (16) 


Equation (15) correlates to the robot process model (4) based on its control input (u), previous state 
(Ry) and noise model (N) while N is equal to zero as the noise in this process is modeled in as white noise. 


b. Robot covariance updating 

The updated covariance P in this process based on model prediction is calculated as in (17), where 
corresponding to the process model equation, Fp is the Jacobian of the state and Pn is the measurement input 
control noise covariance. 


P < FpPFp! + Pn (17) 


As mentioned previously, states of the robot (R) are only affected by the robot movement so the 
covariance matrix P related to the robot states affects the Jacobian matrix to update. Therefore, in this 
process, the Jacobian matrix is calculated as in (18), in which O corresponds to zero matrices and I 
correspond to the identity matrix. 


dik o dfr 
F, = |dR | Ph = a (18) 
0 I 0 


2.2.3. Step 2. landmark-based observation updating process 

The mobile robot observes around the landmark while it is moving using the laser sensor that 
measures observable landmarks range and bearing related to the robot orientation and position. If the 
observed landmark is already registered to the map, its range and bearing measurement are used to update the 
state's estimation (R) and also its covariance (P). The measurement process and its corresponded covariance 
modeled as in (19) and (20) are independent for each landmark (1). The state updating process based on 
observed landmarks is processed one by one of each landmark. 


Z= yi — hj (Ry, Li) (19) 

Hp= [Hr 0 .. 9 Hi O 0] (20) 
— dhj (Ry, Ly) t — dhj (Ry,L,) 

Hey = SRy ? i Sli (21) 


Based on the Jacobian, see (20) and above measurement model, the updated state process and its 
updated covariance based on a set of (22), (23), (24), (25), and (26) are calculated, in which K in this 
updating process is Kalman gain. 


Z= yi — hy (Ry,Li) (22) 
Z= Hy PHx + R (23) 
K = PH,'Z71 (24) 
R —R-Kz (25) 
P e P — KZKT (26) 
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2.2.4. Step 3. Initialization of landmarks 

When the landmarks for the first time are observed by the robots have not registered on the map. 
Whereas, based on its range and bearing measurement the state of this new landmark is estimated including x 
and y global coordinate corresponding to the robot state R. The new landmark estimated state’s function can 
be exhibit as (27) are calculated as the invert of observation function (h; (R, L;)) while the new landmark 
states corresponding Jacobian, the inverse observation function and the robot states are written as (28). 


Lasa = (Xv, Yn+1) (27) 
_ 0g (Xv Yn+1) . _ 0g (Xv Yn+1) 
Gy, = EED ; Gy = evn (28) 


The new landmark covariance and cross-covariance are calculated based on (29) and (30) related to 
the prior states. 


XG (29) 


V ~Yn+1 


Pi = Gy, Pi Gx, + G 


Yn+1 
Pix = Gy, Pur Pxyx, (30) 


Based on the estimation result, these new landmark states and their covariance are then summed into 
the robot full state, map, and covariance as in (31). 





P By? 
P LX 31 
| =i, Bi eo 


3. RESULTS AND DISCUSSION 

To analyze the performance of EKF-SLAM algorithm for autonomous mobile robot navigation 
realization, indoor environment with static and dynamic objects was designed on V-REP simulator that was 
connected with MATLAB via remote API features while two-wheeled differential drive robot i.e., Pioneer 3- 
DX integrated with laser range finder 1.e., Hokuyo URG-04LX-UGOI to determine its performance 
effectiveness with the integration of LIDAR and ultrasonic sensors. EKF- SLAM algorithm is developed 
using MATLAB that is linked to V-REP via remote API feature. To evaluate EKF-SLAM performance. 
Figure 3 shows an environment with static objects designed on V-REP and Figure 4 shows an environment 
with dynamic objects designed on V-REP. 





Figure 3. The environment with static objects Figure 4. The environment with dynamic objects 
designed on V-REP designed on V-REP 


The simulation was performed on V-REP simulator settings 1.e., simulation time step dt = 50 ms, 
dynamics engine = bullet 2.78, dynamic settings=accurate. When the simulation begins mobile robot starts 
scanning with the 2D laser range finder “Hokuyo URG-04LX-UG01” that allows a wide scanning range of 
5600mmx240° for measurement of the landmarks in the indoor environment that integrated into the robot 
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and avoid obstacles with the help of EKF- SLAM algorithm. In an experiment, the exploration is simulated in 
a room bounded by 240 cm high walls, 80 cm walls in the center, chair, table, cupboard, and rack by a 
Pioneer 3-DX robot equipped with a mid-noise and odometry 2D range scanner 1.e., Hokuyo URG-04LX- 
UGO1. Gaussian noise distributed in polar coordinates. Iterative closet point (ICP) algorithm is used for data 
association [16] i.e., performed at each step and exploration performed with Braintenberg, Lyapunov, 
ZigZag, and cornering algorithms [17] which maximize performance. It has been observed that uncertainty is 
the critical measurement parameter of performance degradation which affects measurement models and 
motion due to its direct influence consistency. Figure 5 shows scanning with static objects on the V-REP 
simulator and Figure 6 shows scanning with dynamic objects on the V-REP simulator. 





Figure 5. Scanning with static objects on V-REP Figure 6. Scanning with dynamic objects on V-REP 
simulator simulator 


Graphical user interface (GUI) designed on MATLAB which was interconnected with V-REP 
simulator. As the simulation starts, robot motion navigation is tracked on the V-REP simulator and MATLAB 
GUI simultaneously. The landmarks are pointed with a red cross while the robot was represented with a small 
triangle. Run time set at 3000 s. Figure 7 shows MATLAB GUI for robot navigation. 
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Figure 7. MATLAB GUI for robot navigation 


IAES Int J Rob & Autom, Vol. 10, No. 4, December 2021: 296 — 307 


Int J Rob & Autom ISSN: 2722-2586 g 303 


As the robot moves it find observed landmarks, avoids obstacles, updates states and covariance 
matrices based on the current map and observation. If any new landmarks are found, it initializes updated 
states and covariance. While the robot motion-tracked on a grid map, as shown in Figure 8 and Figure 9. 

Grid maps are introduced in 1985 by Moravec and Elf, that represents environment by a grid, 
assuming robot position is known and occupancy of an individual cell is independent stores the posterior 
probability that a location or corresponding area is occupied by an obstacle, the larger value represents 
obstacle marked by black color and the smaller value represents free space marked by white color. 

Accurate mapping can be achieved by combining lots of data while each cell in the grid represents a 
bit of the robot’s environment indicate some measure of “obstacle Ness” in each grid cell based on laser sensor 
readings while the algorithm operates if the sensor data has been obtained directly from a laser scanner and 
using only the odometry [18]-[20]. 





Figure 8. Grid map with static objects Figure 9. Grid map with dynamic objects 


In the robot navigation view, green lines represent lidar sensor exploration, red and blue triangles 
indicate a mobile robot, blue circles represent landmarks and the Red Cross indicates estimated landmarks, as 
shown in Figure 10 and Figure 11. 





-5 Robot Navigation View 5 5 Robot Navigation View 5 


Figure 10. Navigation view with static objects Figure 11. Navigation view with dynamic objects 


The performance of the proposed EKF-SLAM algorithm in the indoor environment with static and 
dynamic objects was observed and compared in terms of the standard deviation of the vehicle heading and it 
was observed that the uncertainty in position over time is better than usual SLAM results exhibit in Figure 12 
and Figure 13. It was observed that the uncertainty in position over time was observed by plotting the 
standard deviation of position (m) on the y-axis and time(s) on the x-axis, as shown in Table 1. 
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EKF-SLAM: Uncertainity in Position over time EKF-SLAM: Uncertainity in Position over time 
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Figure 12. Uncertainty in position over time with Figure 13. Uncertainty in position over time with 
static objects dynamic objects 


Table 1. Simulated uncertainty in position over time with static and dynamic objects 


Wucerainiy imposition overtime With static objects With dynamic objects 
Position(m) Time(s)  Position(m) Time (s) 
Maximum standard deviation in x 0.0278 612 0.02455 615 
Minimum standard deviation in x 0.0175 255 0.01693 2410 
Maximum standard deviation in y 0.0258 425 0.02145 77 
Minimum standard deviation in y 0.0163 2917 0.00987 2412 


EKF-SLAM performance and accuracy can also be estimated by plotting errors in position over 
time. Uncertainty in robot navigation can be computed by considering noise and Jacobians. Figure 14 and 
Figure 15 represent the error in position over time in the designed indoor environment with static and 
dynamic objects respectively. It was observed at turning positions inside the room there are spikes after a 
certain instant of time and more spikes were observed in simulation with dynamic objects at a certain instant 
of time. In our controlled simulation environment, we have compared the performance of the map joining and 
the consistency of the mapping algorithm while for some trajectory points discrepancies are observed furthest 
from the initial location of the vehicle. 

When mobile robot heading position is estimated by odometry, Figure 16 and Figure 17 represents 
scan error over time by plotting position error (m) and angular error (rad) on the y-axis versus time on the 
x-axis, while odometry error over time represents in Figure 18 and Figure 19. The mobile robot corrects its 
position by updating landmarks. 

Simulation with static objects and dynamic objects results are summarized in Table 2. It can be 
analyzed that fewer scan and odometry errors in 3000 seconds simulation time observed with dynamic 
objects as compared to static objects, although it analyzed that both simulations performed accurate 
navigation using EKF- SLAM algorithm. 


EKF-SLAM: Error in position over time 0.36 EKF-SLANE Error in position over time 
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Figure 14. Error in position over time with static Figure 15. EKF-SLAM-error in position over time 


IAES Int J Rob & Autom, Vol. 10, No. 4, December 2021: 296 — 307 


Int J Rob & Autom ISSN: 2722-2586 g 305 


objects with dynamic objects 


z Scan errors over time 
Scan errors over time i E S O O mm — R a 


=) 

© 

A 
ae 


Angular 
Position 
Error (m) 
S 
+ 
Angular 
Error (rad) 











Position 
Error (m) 








[=] 

© 

oO 
— 





TO NTR 
EA LARTA ai, MN N l 


| i 
| nt ia l 





aha l | 






i | | } \ | 
(0) Laal aab a aeih aiaa, dlas! Jullisa ddl lhd anal dand eala alh hh gul 0 0 akah abd siti i Ibe. bi bu li witilun i dia dile, ' tadh ual) AGT bl iu ud wil id Lill A 
0 500 1000 1500 2000 2500 3000 0 500 1000 1500 2000 2500 3000 
Time (s) Time (s) 
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Figure 16. Scan errors over time with static objects 
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Table 2. Simulation results of EKF-SLAM algorithm 
With static objects With dynamic objects 
Position error Angular error Position error Angular error 
E ah 0.134m at 857 sec 0.0002281 rad at 1017 sec 0.08034 m at 2944 sec 0.0016214 rad at 2757 sec 
errors over time 
ee 0.056 mat 1907 sec 0.0939 rad at 1769 sec 0.0044478 m at 2972 sec 0.004478 rad at 2972 sec 
errors Over time 
Maximum 
odometry errors 0.13317 mat 1084 sec 0.090028 rad at 843 sec 0.13412 m at 857 sec 0.093917 rad at 1769 sec 
over time 


Minimum odometry 9.95591 mat 828 sec 0.000158 rad at 2633 sec 0.05609 mat 1907 sec 0.000477 rad at 1197 sec 
errors over time 


4. CONCLUSION 

This research represents EKF-SLAM algorithm performance in an indoor environment designed 
with static and dynamic objects on a V-REP simulator. It was observed and verified with results that 
autonomous mobile robot navigates using EKF-SLAM algorithm implemented on MATLAB while runtime 
robot navigation viewed on V-REP which was later linked with MATLAB GUI via remote API commands. 
The Pioneer 3-DX Mobile robot i.e., equipped with the Lidar and Ultrasonic sensors performed exploration 
with static and dynamic objects, it was observed through Landmarks measurement that EKF- SLAM corrects 
odometry error. The simulation verifies that the degradation in odometry error and error in position over time 
which in results decrease uncertainty in position over time. The robot defines its path by avoiding obstacles 
and update landmarks if any new landmarks are found during the scan. EKF-SLAM performance observed 
that the robot performs navigation with less position error, scan error, and odometry error in the indoor 
environment with static objects as compared to dynamic objects. The suggested EKF- SLAM solves the 
online SLAM problem by using a linearized Gaussian probability distribution method. It was supposed as 
first probabilistic SLAM algorithm. Data association is one of the challenging problems in navigation in 
which association between measurements and features is unknown that is solved using EKF- SLAM. 

Future work is to estimate algorithm performance for a longer period with reducing computational 
complexity, through experiments and compare its result with current research findings. However, EKF- 
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SLAM with consistent information for a longer period is still recommended for navigation for its stable 
results in complex environments and accurate performance. 
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